RJD-1930 Add see_around for WalkStraightAction #53

gmajrobotec · 2025-09-26T08:11:14Z

Description

This PR implements see_around functionality for WalkStraightAction of npc pedestrian.

Abstract

WalkStraightAction::doAction takes into account see_around parameter and if pedestrian_ignore_see_around is also set to aware before changing pedestrian position it checks if there is no other entity in front of it in distance it can stop.

Details

Check if any entity is in front of pedestrian.
Check if it is in defined minimal distance => minimal distance is calculated based on distance NPC needs to stop.
Check if it will collide with pedestrian.

Destructive Changes

--

Known Limitations

--

References

RJD-1930

dmoszynski · 2025-09-29T12:10:14Z

...tion/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp


 namespace entity_behavior
 {
+enum class SeeAroundMode { blind, aware };


I'm not sure, but 'see around' might also apply to other entities. Maybe we should move this enum class SeeAroundMode to ActionNode since it's being moved anyway?

this part is only for pedestrian_ignore_see_around

dmoszynski · 2025-09-29T12:14:41Z

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

+bool checkPointIsInfront(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point)
+{
+  const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;
+  const auto dx = point.x - pose.position.x;
+  const auto dy = point.y - pose.position.y;
+  const auto vec_yaw = std::atan2(dy, dx);
+  const auto yaw_diff = std::atan2(std::sin(vec_yaw - self_yaw), std::cos(vec_yaw - self_yaw));
+
+  return std::fabs(yaw_diff) <= boost::math::constants::half_pi<double>();
+}
+


Suggested change

bool checkPointIsInfront(

const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point)

{

const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;

const auto dx = point.x - pose.position.x;

const auto dy = point.y - pose.position.y;

const auto vec_yaw = std::atan2(dy, dx);

const auto yaw_diff = std::atan2(std::sin(vec_yaw - self_yaw), std::cos(vec_yaw - self_yaw));

return std::fabs(yaw_diff) <= boost::math::constants::half_pi<double>();

}

bool isPointInFront(

const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point)

{

const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;

const auto angle_to_target_point = std::atan2(target_point.y - pose.position.y, target_point.x - pose.position.x);

const auto yaw_difference = std::atan2(std::sin(angle_to_target_point - self_yaw), std::cos(angle_to_target_point - self_yaw));

return std::fabs(yaw_difference) <= boost::math::constants::half_pi<double>();

}

dmoszynski · 2025-09-29T12:32:12Z

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

+bool WalkStraightAction::detectObstacleInFront(const bool see_around) const
+{
+  if (should_respect_see_around == SeeAroundMode::blind) {
+    return false;
+  }
+
+  if (!see_around) {
+    return false;
+  }
+
+  auto hasObstacleInFrontOfPedestrian = [this](double distance) {
+    using math::geometry::operator-;
+    using math::geometry::operator*;
+
+    const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();
+
+    for (const auto & [_, entity_status] : other_entity_status_) {
+      const auto & other_position = entity_status.getMapPose().position;
+      const auto norm = math::geometry::norm(other_position - pedestrian_pose.position);
+      if (norm > distance) continue;
+
+      if (checkPointIsInfront(pedestrian_pose, other_position)) {
+        // is in front of pedestrian check if collide
+        const auto bounding_box_map_points = math::geometry::transformPoints(
+          pedestrian_pose,
+          math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));
+
+        std::vector<geometry_msgs::msg::Point> front_points;
+        std::vector<geometry_msgs::msg::Point> check_area_points;
+        for (const auto & point : bounding_box_map_points) {
+          if (checkPointIsInfront(pedestrian_pose, point)) {
+            front_points.push_back(point);
+          }
+        }
+        auto orientation_vector =
+          math::geometry::normalize(
+            math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) *
+          distance;
+        for (auto & point : front_points) {
+          check_area_points.push_back(point);
+          point.x += orientation_vector.x;
+          point.y += orientation_vector.z;
+          point.z += orientation_vector.y;
+          check_area_points.push_back(point);
+        }
+        const auto check_polygon = math::geometry::toBoostPolygon(check_area_points);
+        const auto poly =
+          math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());
+        if (
+          boost::geometry::intersects(check_polygon, poly) ||
+          boost::geometry::intersects(poly, check_polygon)) {
+          return true;
+        } else if (boost::geometry::disjoint(check_polygon, poly)) {
+          return false;
+        }
+        return true;
+      }
+    }
+    return false;
+  };
+
+  const double min_stop = calculateStopDistance(behavior_parameter_.dynamic_constraints);
+  const double stable_distance =
+    min_stop + canonicalized_entity_status_->getBoundingBox().dimensions.x;
+
+  return hasObstacleInFrontOfPedestrian(stable_distance);
+}
+


In general, the names need to be improved to make them more informative - they should refer to what is actually being calculated (what the object contains).

In general, you always need to remember {} for if statements, const for immutable objects, and some relative step-by-step logic.

What is suggested here is only part of the changes; the whole thing needs to be reconsidered.

Suggested change

bool WalkStraightAction::detectObstacleInFront(const bool see_around) const

{

if (should_respect_see_around == SeeAroundMode::blind) {

return false;

}

if (!see_around) {

return false;

}

auto hasObstacleInFrontOfPedestrian = [this](double distance) {

using math::geometry::operator-;

using math::geometry::operator*;

const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();

for (const auto & [_, entity_status] : other_entity_status_) {

const auto & other_position = entity_status.getMapPose().position;

const auto norm = math::geometry::norm(other_position - pedestrian_pose.position);

if (norm > distance) continue;

if (checkPointIsInfront(pedestrian_pose, other_position)) {

// is in front of pedestrian check if collide

const auto bounding_box_map_points = math::geometry::transformPoints(

pedestrian_pose,

math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));

std::vector<geometry_msgs::msg::Point> front_points;

std::vector<geometry_msgs::msg::Point> check_area_points;

for (const auto & point : bounding_box_map_points) {

if (checkPointIsInfront(pedestrian_pose, point)) {

front_points.push_back(point);

}

}

auto orientation_vector =

math::geometry::normalize(

math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) *

distance;

for (auto & point : front_points) {

check_area_points.push_back(point);

point.x += orientation_vector.x;

point.y += orientation_vector.z;

point.z += orientation_vector.y;

check_area_points.push_back(point);

}

const auto check_polygon = math::geometry::toBoostPolygon(check_area_points);

const auto poly =

math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());

if (

boost::geometry::intersects(check_polygon, poly) ||

boost::geometry::intersects(poly, check_polygon)) {

return true;

} else if (boost::geometry::disjoint(check_polygon, poly)) {

return false;

}

return true;

}

}

return false;

};

const double min_stop = calculateStopDistance(behavior_parameter_.dynamic_constraints);

const double stable_distance =

min_stop + canonicalized_entity_status_->getBoundingBox().dimensions.x;

return hasObstacleInFrontOfPedestrian(stable_distance);

}

bool WalkStraightAction::detectObstacleInFront(const bool see_around) const

{

auto isObstacleInFrontOfPedestrian = [this](double detection_horizon) {

using math::geometry::operator-;

using math::geometry::operator*;

const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();

for (const auto & [_, entity_status] : other_entity_status_) {

const auto & other_position = entity_status.getMapPose().position;

if (const auto distance = math::geometry::norm(other_position - pedestrian_pose.position); distance <= detection_horizon) {

if (isPointInfront(pedestrian_pose, other_position)) {

/// @note if in front of pedestrian -> check if collide

const auto bounding_box_map_points = math::geometry::transformPoints(pedestrian_pose, math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));

std::vector<geometry_msgs::msg::Point> bounding_box_front_points;

for (const auto & point : bounding_box_map_points) {

if (isPointInfront(pedestrian_pose, point)) {

bounding_box_front_points.push_back(point);

}

}

/// @attention rename orientation_vector...

const auto orientation_vector = math::geometry::normalize(math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) * detection_horizon;

/// @attention [reply on github] Why is the point added twice to check_area_points?

std::vector<geometry_msgs::msg::Point> pedestrian_check_area_points;

for (auto & point : pedestrian_front_points) {

pedestrian_check_area_points.push_back(point);

point.x += orientation_vector.x;

point.y += orientation_vector.z;

point.z += orientation_vector.y;

pedestrian_check_area_points.push_back(point);

}

const auto pedestrian_check_area_polygon = math::geometry::toBoostPolygon(pedestrian_check_area_points);

const auto other_entity_polygon = math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());

if (boost::geometry::intersects(pedestrian_check_area_polygon, other_entity_polygon) or boost::geometry::intersects(other_entity_polygon, pedestrian_check_area_polygon)) {

return true;

} else if (boost::geometry::disjoint(pedestrian_check_area_polygon, other_entity_polygon)) {

return false;

} else {

return true;

}

}

}

}

return false;

};

if (not see_around or should_respect_see_around == SeeAroundMode::blind) {

return false;

} else {

const double detection_horizon = calculateStopDistance(behavior_parameter_.dynamic_constraints) + canonicalized_entity_status_->getBoundingBox().dimensions.x;

return isObstacleInFrontOfPedestrian(detection_horizon);

}

}

Changed names and restructured function.

dmoszynski · 2025-09-29T12:32:39Z

simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp

    target_speed_ = 1.111;
  }
+
+  const auto obstacle_detector_result = detectObstacleInFront(behavior_parameter_.see_around);


The name “obstacle_detector_result” suggests that it is a complex object, but I don't think it is, is it?

changed name

RJD-1930 Add see_around for WalkStraightAction

e7e2c56

gmajrobotec requested review from piotr-zyskowski-rai and dmoszynski September 26, 2025 08:11

dmoszynski reviewed Sep 29, 2025

View reviewed changes

Changed names and structure

b37c32e

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

RJD-1930 Add see_around for WalkStraightAction #53

RJD-1930 Add see_around for WalkStraightAction #53

Uh oh!

gmajrobotec commented Sep 26, 2025 •

edited by dmoszynski

Loading

Uh oh!

dmoszynski Sep 29, 2025

Uh oh!

gmajrobotec Sep 30, 2025

Uh oh!

dmoszynski Sep 29, 2025

Uh oh!

gmajrobotec Sep 30, 2025

Uh oh!

dmoszynski Sep 29, 2025

Uh oh!

gmajrobotec Sep 30, 2025

Uh oh!

dmoszynski Sep 29, 2025

Uh oh!

gmajrobotec Sep 30, 2025

Uh oh!

Uh oh!

RJD-1930 Add see_around for WalkStraightAction #53

Are you sure you want to change the base?

RJD-1930 Add see_around for WalkStraightAction #53

Uh oh!

Conversation

gmajrobotec commented Sep 26, 2025 • edited by dmoszynski Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Abstract

Details

Destructive Changes

Known Limitations

References

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

gmajrobotec commented Sep 26, 2025 •

edited by dmoszynski

Loading